﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace PayloadTracker.APRS.SoundModem
{
    public class AX25Serializer
    {
        public AX25Serializer()
        {
        }
        public byte[] Serialize(AX25Packet packet)
        {
            List<Byte> bytes = new List<byte>(); // output array

            //add first call sign 
           bytes.AddRange(ShiftBits(-1,System.Text.ASCIIEncoding.ASCII.GetBytes(CorrectString(6, packet.CallsignFrom))));

           int ssid1 = 7; //111b
           ssid1 = ssid1 << 4; //1110000b
           ssid1 = ssid1 | Int32.Parse(packet.SSIDFrom); //1110000b
           ssid1 = ssid1 << 1; //111SSID0b
           bytes.Add((byte)ssid1);

           //add second call sign 
           bytes.AddRange(ShiftBits(-1,System.Text.ASCIIEncoding.ASCII.GetBytes(CorrectString(6, packet.CallsignTo))));

           int ssid2 = 3; //011b
           ssid2 = ssid2 << 4; //0110000b
           ssid2 = ssid2 | Int32.Parse(packet.SSIDTo); //0110000b
           ssid2 = (ssid2 << 1) + 1; //111SSID1b
           bytes.Add((byte)ssid2);

           
            //add AX25 control byte
           bytes.Add(packet.Control);

            //add ax25 packet id.
           bytes.Add(packet.PID);

            //add the packet data
           bytes.AddRange(packet.Data);


            //compute CRC
            var crc = GenerateCRC_CCITT(bytes);
            System.Diagnostics.Debug.WriteLine("SS CRC: " + crc.ToString());
            
            //add squelch header
            AddSquelch(10, bytes, true);

            //add the CRC
            bytes.AddRange(System.BitConverter.GetBytes(crc));
            
            //add squelch tail.
            AddSquelch(10, bytes, false);



            return bytes.ToArray();
        }
        //Shifts bits of a byte array 
        private IEnumerable<Byte> ShiftBits(int dir, IEnumerable<Byte> array)
        {
            foreach(byte b in array)
            {
                if (dir > 0)
                    yield return ((byte)(b >> dir));
                else if (dir < 0)
                    yield return ((byte)(b << Math.Abs(dir)));
                else
                    yield return b;
            }
        }
        //Pads the tail of a string to a specified length.
        private string CorrectString(int to,string s)
        {
            while (s.Length < to)
                s += " ";
            return s;
        }
        //Adds len number of FLAG bytes to an array at either the front or back of an array.
        private void AddSquelch(int len, List<Byte> bytes, bool front)
        {
            for (int i = 0; i < len; i++)
                if (front)
                    bytes.Insert(0, (byte)AX25Deserializer.CONSTS.SEPARATOR);
                else
                    bytes.Add((byte)AX25Deserializer.CONSTS.SEPARATOR);
        }

        //computes CRC
        private ushort GenerateCRC_CCITT(List<Byte> bytes)
        {
            ushort computedcrc = 0xffff;
            for (int i = 0; i < bytes.Count; i++)
            {
                byte c = bytes[i];
                for (byte bit = 8; bit != 0; bit--)
                {
                    if (((computedcrc ^ c) & 1) != 0)
                        computedcrc = ((ushort)((computedcrc >> 1) ^ 0x8408));
                    else
                        computedcrc = ((ushort)(computedcrc >> 1));
                    c = ((byte)(c >> 1));
                }
            }
            computedcrc = ((ushort)~computedcrc);
            return computedcrc;
        }

    }
}
